#include <devices/sysclk.h>
#include <devices_ext/mti.h>
#include <devices_ext/promotion.h>
#include "tasks.h"
#include "tasks/findline.h"
#include "tasks/hitwall.h"
#include "tasks/choices_api.h"

/* 机器人在不同阶段的动作 */
static void act1();
static void act2();

/*+ 消息处理器 */
static void find_line( pSysMsg m );
static void hit_wall( pSysMsg m );
static SysMsgHandler handlers[NR_CCMIDS] = {
		find_line,
		hit_wall,
};
#include <leaf/sysmsghandler.h>
/*- 消息处理器 */

MsgBox choices_mb;
static SysMsg motion_cmd;
static Timer tmr;

/* 主任务体 */
DEF_TASK( choices, 256, 1 )
{
	/* x方向用串口0, 左右分别编号为1,2 baud=115200 */
	prom_init( &xl, 0, 1 );
	prom_init( &xr, 0, 2 );

	/* y方向用串口1, 左右分别编号为3,4 baud=115200 */
	prom_init( &yl, 0, 3 );
	prom_init( &yr, 0, 4 );

	fl_init();

	act1();

	while( true ){
		pSysMsg m = wait_sys_msg( &choices_mb );
		handle_sys_msg( m );
	}
	sleep_ticks( &tmr, 1231 );
}

static void act1()
{
	prom_speed( &xl, 3000 );
	prom_speed( &xr, 3000 );

	prom_speed( &yl, 3000 );
	prom_speed( &yr, 3000 );

	fl_start();
	set_handler( FindLine, find_line );
}

static void act2()
{
//	resume_task( motion );        //让motion任务接管驱动器的控制
	post_max_speed( &motion_cmd, 4000 );

	hw_start();
	set_handler( HitWall, hit_wall );
}

static void find_line( pSysMsg m )
{
	fl_stop();
	reset_handler( FindLine );

	prom_speed( &yl, 0 );
	prom_speed( &yr, 0 );

	act2();
}

static void hit_wall( pSysMsg m )
{
	hw_stop();
	post_max_speed( &motion_cmd, 0 );
}

/* APIs */
void post_choice_msg( SysMsg *msg, CCMid id )
{
	msg->id = id;
	send_sys_msg( &choices_mb, msg );
}

